/*
ASCII Serial Commands
 All 2 byte pairs, except for STOP, GET_VERSION, and CAMRELEASE
 
 FORWARD = 'f', [0-255] (speed) 
 BACKWARD = 'b', [0-255] (speed)
 LEFT = 'l', [0-255] (speed)
 RIGHT = 'r', [0-255] (speed)
 CAM = 'v', [0-255] (servo angle)  
 ECHO ON = 'e', '1' (echo command back TRUE)
 ECHO OFF = 'e', '0' (echo command back FALSE)
 STOP = 's' (DC motors stop)
 GET VERSION = 'y'
 GET FIRMWARE = 'x'
 STEPPER FIND HOME = 'm'
 STEPPER FORWARD = 't'
 STEPPER REAR = 'z'
 STEPPER TO POSITION = 'v'
 BATTERY VOLTS = 'p'
 DOCK VOLTS = 'q'
 DOCKFET HIGH = 'i' // testing
 DOCKFET LOW = 'o' // testing
*/

// for ebay bridge 
const int pwmA = 3;
const int in1 = 4;
const int in2 = 5;
const int pwmB = 11;
const int in3 = 9;
const int in4 = 10;

// buffer the command in byte buffer 
const int MAX_BUFFER = 8;
int buffer[MAX_BUFFER];
int commandSize = 0;
boolean echo = false;

// stepper motor for camera tilt 
const int HOME_STEPS = 1500; // approx max number of steps for full range, plus some extra
const int STEPS_PER_DEGREE = 6; // 
int stepperPosition = 0; 
int stepperTarget = 0;
int stepperPhase = 0; /* (0-3) full step, or (0-7) half step */
int nextStepperTarget = 0;
boolean calibrationMode = false;
const int EXTRA_STEPS = 500; // periodic calibration steps to find front or rear stops
int lastReportedPosition = stepperPosition;

// stepper pins 
const int stepPin0 = 13; // changed from 12
const int stepPin1 = 8; 
const int stepPin2 = 2; 
const int stepPin3 = 6; 

// dock charge mosfet
const int dockFetPin = 12; 

// analog input pins
const int battVoltsInput  = 1;
const int dockVoltsInput = 3;

// timers 
unsigned long lastcmd = 0;
unsigned long laststep = 0;
// unsigned long lastbattery = 0;
unsigned long lastdockcheck = 0;

// const int BATTERY_DELAY = 9000; // just testing, polling from java currently 
const int TIMEOUT = 9000;
const int STEP_DELAY = 4;
const int CHECK_DOCK_WHEN_UNDOCKED = 1000; 
const int CHECK_DOCK_WHEN_DOCKED = 20; // faster, to close fet quickly and reduce arcing on reconnect
int checkIfDockedDelay = CHECK_DOCK_WHEN_UNDOCKED;
boolean docked = false;
double dockVoltsThreshold = 12.0; // will vary with battery chemistry, diode/fet forward voltages

//
//
//
void setup() { 
  
	pinMode(in1, OUTPUT); 
	pinMode(in2, OUTPUT); 
	pinMode(in3, OUTPUT); 
	pinMode(in4, OUTPUT); 

	pinMode(stepPin0, OUTPUT);  
	pinMode(stepPin1, OUTPUT);  
	pinMode(stepPin2, OUTPUT);  
	pinMode(stepPin3, OUTPUT);  
	
	pinMode(dockFetPin, OUTPUT);
	digitalWrite(dockFetPin, LOW);
	
	TCCR2B = TCCR2B & 0b11111000 | 0x07;  // 30 Hz pin 3, 11
	
	Serial.begin(115200);
	Serial.println("<reset>"); 
	
 }

//
//
//
void loop(){
  
	if( Serial.available() > 0 ){
		// commands take priority 
		lastcmd = millis();
		manageCommand(); 
	} 

	if (millis() - lastcmd > TIMEOUT){ 
		// if no comm with host, stop motors
		Serial.println("<timeout>"); 
		analogWrite(pwmA, 0); 
		analogWrite(pwmB, 0);
		// OCR2A = 0;
		// OCR2B = 0;
		lastcmd = millis(); 
	}

	if(stepperPosition != stepperTarget){
		// motor tracking in to new target
		if (millis() - laststep > STEP_DELAY){

			if(stepperPosition > stepperTarget){
				stepperPosition--;  
				stepperPhase++; 
				if(stepperPhase > 3) stepperPhase = 0; 
				fullStep(); 
			} else {
				stepperPosition++;
				stepperPhase--;
				if(stepperPhase < 0) stepperPhase = 3; 
				fullStep(); 
			}

			laststep = millis();
		}  
	}
  
	if(stepperPosition == stepperTarget)  {
		stepperOff();
		if (lastReportedPosition != stepperPosition) {
			Serial.print("<tiltpos ");
			Serial.print(stepperTarget/STEPS_PER_DEGREE);
			Serial.println(">");
			lastReportedPosition = stepperPosition;
		}
		if (calibrationMode == true) {
			stepperPosition = nextStepperTarget;
			stepperTarget = nextStepperTarget;
			calibrationMode = false;
		}
	}
  
	// if (millis() - lastbattery > BATTERY_DELAY) {
		// battVolts();
		// lastbattery = millis();
	// }
	
	if (millis() - lastdockcheck > checkIfDockedDelay) {
		checkDock();
		lastdockcheck = millis();
	}

}

//
//
//
 void fullStep(){ 
 switch(stepperPhase){ 
   case 0: 
     digitalWrite(stepPin0, LOW);  
     digitalWrite(stepPin1, LOW); 
     digitalWrite(stepPin2, HIGH); 
     digitalWrite(stepPin3, HIGH); 
   break;  
   case 1: 
     digitalWrite(stepPin0, LOW);  
     digitalWrite(stepPin1, HIGH); 
     digitalWrite(stepPin2, HIGH); 
     digitalWrite(stepPin3, LOW); 
   break;  
   case 2: 
     digitalWrite(stepPin0, HIGH);  
     digitalWrite(stepPin1, HIGH); 
     digitalWrite(stepPin2, LOW); 
     digitalWrite(stepPin3, LOW); 
   break;  
   case 3: 
     digitalWrite(stepPin0, HIGH);  
     digitalWrite(stepPin1, LOW); 
     digitalWrite(stepPin2, LOW); 
     digitalWrite(stepPin3, HIGH); 
   break;  
 } 
}

//
// turn off tilt motor
//
void stepperOff(){
  digitalWrite(stepPin0, LOW);  
  digitalWrite(stepPin1, LOW); 
  digitalWrite(stepPin2, LOW); 
  digitalWrite(stepPin3, LOW);  
}

//
// Blocking, only call at startup !!
//
void homeMotor(){
  
	for(int  i = 0; i < HOME_STEPS; i++ ){
		stepperPhase++; 
		if(stepperPhase > 3) stepperPhase = 0; 
		fullStep();
		delay(STEP_DELAY); 
	}
 
  // reset  
  stepperOff();
  stepperPosition = 0; 
  stepperTarget = 0;
  stepperPhase = 0;
  // Serial.println("<tilt home>"); 
}

//
// buffer and/or execute commands from host controller 
//
void manageCommand(){
  int input = Serial.read();

  // end of command -> exec buffered commands 
  if((input == 13) || (input == 10)){
    if(commandSize > 0){
      parseCommand();
      commandSize = 0; 
    }
  } else {
    
    // buffer it 
    buffer[commandSize++] = input;

    // protect buffer
    if(commandSize >= MAX_BUFFER){
      commandSize = 0;
    }
  }
}

//
// do multi byte 
//
void parseCommand(){
  
	if (buffer[0] == 'm'){ // find home tilt front stop, blocking
		homeMotor();
		return;
	}

	if (buffer[0] == 't'){ // home tilt front stop
		nextStepperTarget = 0;
		calibrationMode = true;
		stepperTarget = 0-EXTRA_STEPS;
		return;
	}

	if (buffer[0] == 'z'){ // home tilt rear stop
		nextStepperTarget = STEPS_PER_DEGREE*(int)buffer[1];
		calibrationMode = true;
		stepperTarget = (STEPS_PER_DEGREE*(int)buffer[1]) + EXTRA_STEPS;
		return;
	}
  
    if(buffer[0] == 'v') { 
		if (calibrationMode) { 
			calibrationMode=false; 
			stepperPosition = nextStepperTarget;
		}
		stepperTarget = STEPS_PER_DEGREE*(int)buffer[1]; 
	}

	if(buffer[0]== 'w') stepperOff();
  
	// always set speed on each move command 
	if((buffer[0] == 'f') || (buffer[0] == 'b') || (buffer[0] == 'l') || (buffer[0] == 'r')){
		analogWrite(pwmA, buffer[1]); 
		analogWrite(pwmB, buffer[1]);
		// OCR2A = buffer[1]; 
		// OCR2B = buffer[1]; 
	} 
  
  if (buffer[0] == 'b') { // backward
    digitalWrite(in1, LOW);
    digitalWrite(in2, HIGH);
    digitalWrite(in3, LOW);
    digitalWrite(in4, HIGH);
  }

  if (buffer[0] == 'f') { // forward
    digitalWrite(in1, HIGH);
    digitalWrite(in2, LOW);
    digitalWrite(in3, HIGH);
    digitalWrite(in4, LOW);
  }
  
  if (buffer[0] == 'l') { // left
    digitalWrite(in1, LOW);
    digitalWrite(in2, HIGH);
    digitalWrite(in3, HIGH);
    digitalWrite(in4, LOW);
  }

  if (buffer[0] == 'r') { // right
	digitalWrite(in1, HIGH);
    digitalWrite(in2, LOW);
    digitalWrite(in3, LOW);
    digitalWrite(in4, HIGH);
  } 
  
	if (buffer[0] == 's') { // stop
		analogWrite(pwmA, 0); 
		analogWrite(pwmB, 0);
		// OCR2A = 0;
		// OCR2B = 0;
	}
  
  if(buffer[0] == 'x') Serial.println("<id::oculusPrime>");
  
  if(buffer[0] == 'y') Serial.println("<version:0.7>"); 
 
  if(buffer[0] == 'p') battVolts();
  
  if (buffer[0] == 'q')  { docked = !docked;  checkDock(); }
  
  if(buffer[0] == 'i') { digitalWrite(dockFetPin, HIGH); } // testing
  if(buffer[0] == 'o') { digitalWrite(dockFetPin, LOW); } // testing

  if(buffer[0] == 'e') {
    if(buffer[1] == '1'){
      Serial.println("<echo on>");
      echo = true;
    }

    if(buffer[1] == '0'){
      Serial.println("<echo off>");
      echo = false ;
    }
  } 
  
  if(echo) { // echo the command back
    Serial.print("<");
    Serial.print((char)buffer[0]);

    if(commandSize > 1) Serial.print(',');    

    for(int b = 1 ; b < commandSize ; b++){
      Serial.print((String)buffer[b]);  
      if(b<(commandSize-1)) 
        Serial.print(',');    
    } 
    Serial.println(">");
  }
}

void battVolts() {		
	// read voltage0
	int val = analogRead(battVoltsInput);
	// actual measured resistor values, 6800 and 2700 nominal:
	float denominator = (float)2660 / (6740 + 2660); //  R2 / (R1 + R2)   ... R2 = closer to GND
	double volts  = (val / 1023.0) * 5.0; // using nominal Vcc, is not accurate
	volts = volts/ denominator;
	Serial.print("<power ");
	printDouble(volts,1);
	Serial.println(">");
} 

void checkDock() {		
	// read voltage
	int val = analogRead(dockVoltsInput);
	float denominator = (float)2700 / (6800 + 2700); //  R2 / (R1 + R2)   ... R2 = closer to GND
	double volts  = (val / 1023.0) * 5.0; // using nominal Vcc, is not accurate
	volts = volts/ denominator;
	
	if (volts > dockVoltsThreshold && docked == false) {
		digitalWrite(dockFetPin, HIGH); 
		docked = true;
		checkIfDockedDelay = CHECK_DOCK_WHEN_DOCKED;
		Serial.println("<docked>");
	}
	
	if (volts < dockVoltsThreshold && docked == true) {
		digitalWrite(dockFetPin, LOW); 
		docked = false;
		checkIfDockedDelay = CHECK_DOCK_WHEN_UNDOCKED;
		Serial.println("<un-docked>");
	}
	
	// Serial.print("<dockvolts ");
	// printDouble(volts,1);
	// Serial.println(">");
} 

// Print decimal numbers
void printDouble(double val, byte precision) {
	Serial.print (int(val));                                     // Print int part
	if( precision > 0) {                                         // Print decimal part
		Serial.print(".");
		unsigned long frac, mult = 1;
		byte padding = precision -1;
		while(precision--) mult *=10;
		if(val >= 0) frac = (val - int(val)) * mult; else frac = (int(val) - val) * mult;
		unsigned long frac1 = frac;
		while(frac1 /= 10) padding--;
		while(padding--) Serial.print("0");
		Serial.print(frac,DEC) ;
	}
}





